#pragma ident "$Id: SolverIFCBED.cpp 2877 2011-08-20 08:40:11Z yanweignss $"

/**
 * @file SolverIFCBED.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//
//  L5 = rou + ifcb;
//  gfif = LC15 - LC12 = -f5^2/(f1^2-f5^2)*ifcb
//
//============================================================================


#include "SolverIFCBED.hpp"
#include "SystemTime.hpp"

#define debug 0

namespace gpstk
{

      // Index initially assigned to this class
   int SolverIFCBED::classIndex = 9600000;

      // Returns an index identifying this object.
   int SolverIFCBED::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string SolverIFCBED::getClassName() const
   { return "SolverIFCBED"; }

   void SolverIFCBED::Init(void)
   {
       // set up variables and equations for PPP

       //////////////////////////////////////////////////////////////
       //
       // GFIF(1,2,5) = IF(1,2) - IF(1,5) = ifcb + const;
       //
       //////////////////////////////////////////////////////////////

   }

   void SolverIFCBED::setUpEquations(void)
   {
   } // end of setUpEquations



   sourceTypeValueMap& SolverIFCBED::Process(CommonTime& epoch,
                                             const SatID& sat, 
                                             sourceTypeValueMap& srcTypeValueData)
   {
       if(firstTime)
       {
            // insert current diff into epochDiffData for recovering at last
            epochDiffData[epoch] = 0.0;

            // srcArcData setting
            for(sourceTypeValueMap::iterator stvIt=srcTypeValueData.begin();
                    stvIt!=srcTypeValueData.end();
                    stvIt++)
            {
                SourceID source( (*stvIt).first );
                typeValueMap tvData ( (*stvIt).second );

                // set intial value
                if(srcArcData.find(source) == srcArcData.end())
                {
                    srcArcData[source] = 0.0;
                }

                bool csOcurred(false);
                double arcNum = tvData(TypeID::satArc);
                if(srcArcData[source] != arcNum)
                {
                    csOcurred = true;
                    srcArcData[source] = arcNum;
                }
            }

            lastSrcData.clear();

            // store the old data;
            lastSrcData = srcTypeValueData;

            // no longer first time
            firstTime = false;

            return srcTypeValueData;
       }
       else
       {
            std::map<SourceID,double> diffGFIFData;
            for(sourceTypeValueMap::iterator stvIt=srcTypeValueData.begin();
                    stvIt!=srcTypeValueData.end();
                    stvIt++)
            {
                SourceID source( (*stvIt).first );
                typeValueMap tvData ( (*stvIt).second );

                // set intial value
                if(srcArcData.find(source) == srcArcData.end())
                {
                    srcArcData[source] = 0.0;
                }

                bool csOcurred(false);
                double arcNum = tvData(TypeID::satArc);
                if(srcArcData[source] != arcNum)
                {
                    csOcurred = true;
                    srcArcData[source] = arcNum;
                }

                // diffGFIFData
                if(lastSrcData.find( source )!=lastSrcData.end())
                {
                    if(!csOcurred)
                    {
                        typeValueMap lastTvData = lastSrcData[source];
                        double lastGFIF = lastTvData(TypeID::GFIF);
                        double currGFIF = tvData(TypeID::GFIF);
                        double diff = currGFIF - lastGFIF;
                        diffGFIFData[source] = diff;
                    }
                }
            };

            // compute the mean diffGFIF
            double meanDiff(0.0);
            for(std::map<SourceID,double>::iterator srcIt=diffGFIFData.begin();
                    srcIt!=diffGFIFData.end();
                    srcIt++)
            {
                meanDiff += (*srcIt).second;
            }

            // mean diff
            if(diffGFIFData.size() >0 )
            {
                meanDiff = meanDiff/double(diffGFIFData.size());

                // insert current diff into epochDiffData for recovering at last
                epochDiffData[epoch] = meanDiff;

                lastSrcData.clear();

                // store the old data;
                lastSrcData = srcTypeValueData;
            }
            else
            {
                epochDiffData.clear();
                cerr << "SolverIFCBED:: data break ocurred:" << endl;
                exit(-1);
            }

            return srcTypeValueData;
       }
   };

      /// Returns an index identifying this object.
      /// PostProcess consists of backward-filtering and kalman-smoothing.
   void SolverIFCBED::Recover() 
       noexcept(false)
   {

       const double bg15( 1.0           /( GAMMA_GPS_L1L5 - 1.0 ) );
       double ifcb(0.0);
       for(std::map<CommonTime, double>::iterator it=epochDiffData.begin();
               it!=epochDiffData.end();
               it++)
       {
           ifcb += (*it).second;
           epochIFCBData[(*it).first] = ifcb/(-1.0*bg15);
//         epochIFCBData[(*it).first] = ifcb;
       }

   };



   // print float solutions
   void SolverIFCBED::printOut(string file)
   {
       ifcbStream.open(file);
       for(std::map<CommonTime, double>::iterator it=epochIFCBData.begin();
           it!=epochIFCBData.end();
           it++)
       {
           YDSTime time((*it).first);
           double ifcb = (*it).second;

           ifcbStream << time.year << " "
                      << time.doy  << " "
                      << time.sod  << " ";
           ifcbStream << ifcb << " " ;
           ifcbStream << endl;
       }
   }

}  // End of namespace gpstk
